﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace HubertTestProgram
{
    public class Movement: BrainProcess
    {
        #region Fields
        private ServoMotor baseRotationServo;
        private ServoMotor shoulderServo;
        private ServoMotor elbowServo;
        private ServoMotor handServo;
        private ServoMotor headPanServo;
        private ServoMotor headTiltServo;
        private bool initialize; // Starts the initstate when true

        // Used for manipulating and calculating pose
        private short lastCommandToBaseRotationServo;
        private short lastCommandToHeadPanServo;
        private const short BASE_ROTATION_NEUTRAL = 700;
        private const short BASE_ROTATION_RIGHT_90 = 1150;
        private const short BASE_ROTATION_LEFT_90 = 250;
        private const double BASE_ROTATION_DEG_COEFFICIENT = 5.0;
        private const short HEAD_PAN_NEUTRAL = 720;
        private const short HEAD_PAN_RIGHT_90 = 1170;
        private const short HEAD_PAN_LEFT_90 = 270;
        private const double HEAD_PAN_DEG_COEFFICIENT = BASE_ROTATION_DEG_COEFFICIENT; // If != BASE_ROTATION_DEG_COEFFICIENT => change RotateHeadRelativeToCurrentPose

        private const double HEAD_PAN_STEP = 15;
        private const byte HEAD_PAN_SPEED = 5;
        private const byte BASE_ROTATION_SPEED = 4;
        private bool turnLeft;
        #endregion

        #region Private methods
        /* Positive degrees correspond to rotation left.
         * Rotation of head will also rotate base if necessary.
         * Returns true if command is sent. Returns false if
         * command is not possible.
         */
        private bool RotateHeadRelativeToCurrentPose(double degrees)
        {
            //Make sure we only rotate front +/- 90 degrees
            double newAbsoluteHeadPose = this.EstimatedCurrentAbsoluteCameraPose + degrees;
            if (newAbsoluteHeadPose > 95.0 || newAbsoluteHeadPose < -95.0)
            {
                return false;
            }
            else
            {
                short head;
                short body;

                short rotate = (short)(this.lastCommandToHeadPanServo - Math.Round(degrees * HEAD_PAN_DEG_COEFFICIENT));
                if (rotate < HEAD_PAN_LEFT_90)
                {
                    head = HEAD_PAN_LEFT_90;
                    body = (short)(this.lastCommandToBaseRotationServo - (HEAD_PAN_LEFT_90 - rotate));
                }
                else if (rotate > HEAD_PAN_RIGHT_90)
                {
                    head = HEAD_PAN_RIGHT_90;
                    body = (short)(this.lastCommandToBaseRotationServo + (rotate - HEAD_PAN_RIGHT_90));
                }
                else
                {
                    head = rotate;
                    body = this.lastCommandToBaseRotationServo;
                }

                bool bodyRotationOk = body > BASE_ROTATION_LEFT_90 && body < BASE_ROTATION_RIGHT_90;
                bool headPanOk = head > HEAD_PAN_LEFT_90 && head < HEAD_PAN_RIGHT_90;

                if (bodyRotationOk && headPanOk)
                {
                    Robot.Instance.Body.BasicStamp.SetServoPosition(baseRotationServo, body, BASE_ROTATION_SPEED);
                    this.lastCommandToBaseRotationServo = body;

                    Robot.Instance.Body.BasicStamp.SetServoPosition(headPanServo, head, HEAD_PAN_SPEED);
                    this.lastCommandToHeadPanServo = head;

                    return true;
                }
                else
                {
                    return false;
                }          
            }
        }

        /* 0 degrees corresponds to facing front.
         * Positive degrees correspond to left facing poses.
         * Rotation of base will not change the absolute pose of head.
         * Returns true if command is sent. Returns false if
         * command is not possible.
         */
        private bool RotateBaseToAbsolutePose(double degrees)
                                                        //60
        {
            short body = (short)(BASE_ROTATION_NEUTRAL - Math.Round(degrees * BASE_ROTATION_DEG_COEFFICIENT));
                    //400  = 700 - 5*60
            short head = (short)(this.lastCommandToHeadPanServo + (this.lastCommandToBaseRotationServo - body));
                    //720 = (720 - 5*60) + (700 - 400)

            bool bodyRotationOk = body > BASE_ROTATION_LEFT_90 && body < BASE_ROTATION_RIGHT_90;
            bool headPanOk = head > HEAD_PAN_LEFT_90 && head < HEAD_PAN_RIGHT_90;

            if (bodyRotationOk && headPanOk)
            {
                Robot.Instance.Body.BasicStamp.SetServoPosition(baseRotationServo, body, BASE_ROTATION_SPEED);
                this.lastCommandToBaseRotationServo = body;

                Robot.Instance.Body.BasicStamp.SetServoPosition(headPanServo, head, HEAD_PAN_SPEED);
                this.lastCommandToHeadPanServo = head;

                return true;
            }
            else
            {
                return false;
            }
        }
        #endregion

        #region Public methods
        public Movement()
        {
            // Only for Manual Control
            this.ManualControl = false;
            this.ManualServoBase     = 750; //Netral 700
            this.ManualServoShoulder = 750; //Netral 1110
            this.ManualServoElbow    = 750; //Straight 740
            this.ManualServoHand     = 750; //Closed 1000, Open 600
            this.ManualServoPan      = 750; //Netral 720
            this.ManualServoTilt     = 750; //Netral 720

            // Just references, introduced for brevity..
            baseRotationServo = Robot.Instance.Body.BaseRotationServo;
            shoulderServo = Robot.Instance.Body.ShoulderServo;
            elbowServo = Robot.Instance.Body.ElbowServo;
            handServo = Robot.Instance.Body.HandServo;
            headPanServo = Robot.Instance.Body.HeadPanServo;
            headTiltServo = Robot.Instance.Body.HeadTiltServo;

            this.turnLeft = true;
        }

        public override void Initialize()
        {
            base.Initialize();
            this.initialize = true;
        }

        public override void Step()
        {
            if (this.initialize)
            {
                // Set servos to netral position
                Robot.Instance.Body.BasicStamp.SetServoPosition(baseRotationServo, 700, 10);
                this.lastCommandToBaseRotationServo = 700;
                Robot.Instance.Body.BasicStamp.SetServoPosition(shoulderServo, 1110, 10);
                Robot.Instance.Body.BasicStamp.SetServoPosition(elbowServo, 740, 10);
                Robot.Instance.Body.BasicStamp.SetServoPosition(handServo, 1000, 10);
                Robot.Instance.Body.BasicStamp.SetServoPosition(headPanServo, 720, 10);
                this.lastCommandToHeadPanServo = 720;
                Robot.Instance.Body.BasicStamp.SetServoPosition(headTiltServo, 740, 10);
               
                if (TimeInState > 3.000) 
                {
                    this.initialize = false;
                    this.Utility = -1;
                }
            }
            else if (this.ManualControl)
            {
                Robot.Instance.Body.BasicStamp.SetServoPosition(baseRotationServo, this.ManualServoBase, this.ManualServoSpeed);
                this.lastCommandToBaseRotationServo = this.ManualServoBase;
                Robot.Instance.Body.BasicStamp.SetServoPosition(shoulderServo, this.ManualServoShoulder, this.ManualServoSpeed);
                Robot.Instance.Body.BasicStamp.SetServoPosition(elbowServo, this.ManualServoElbow, this.ManualServoSpeed);
                Robot.Instance.Body.BasicStamp.SetServoPosition(handServo, this.ManualServoHand, this.ManualServoSpeed);
                Robot.Instance.Body.BasicStamp.SetServoPosition(headPanServo, this.ManualServoPan, this.ManualServoSpeed);
                this.lastCommandToHeadPanServo = this.ManualServoPan;
                Robot.Instance.Body.BasicStamp.SetServoPosition(headTiltServo, this.ManualServoTilt, this.ManualServoSpeed);
                if (TimeInState > 2.000)
                {
                    this.Utility = -1;
                }
            }
            else if (CurrentState == 0)
            {
                if (TimeInState > 1.000) // Let head move before looking
                {
                    Robot.Instance.Brain.DetectMovingPerson.Utility = 1;
                }

                if (TimeInState > 4.000)
                {
                    Robot.Instance.Brain.DetectMovingPerson.Utility = -1;
                    if (Robot.Instance.Brain.DetectMovingPerson.PersonDetected)
                    {
                        CurrentState = 1;
                    }
                    else
                    {
                        CurrentState = 2;
                    }
                }
            }
            else if (CurrentState == 1)
            {
                RotateBaseToAbsolutePose(Robot.Instance.Brain.DetectMovingPerson.DirectionToPerson);
                Robot.Instance.Body.BasicStamp.SetServoPosition(shoulderServo, 600, 10);
                Robot.Instance.Brain.TextToSpeechBrainProcess.Utility = 1;
                this.Utility = -1;
            }
            else if (CurrentState == 2)
            {
                if (turnLeft)
                {
                    if (RotateHeadRelativeToCurrentPose(HEAD_PAN_STEP))
                    {
                        CurrentState = 0;
                    }
                    else
                    {
                        turnLeft = false;
                    }
                }
                else
                {
                    if (RotateHeadRelativeToCurrentPose(- HEAD_PAN_STEP))
                    {
                        CurrentState = 0;
                    }
                    else
                    {
                        turnLeft = true;
                    }
                }
            }
        }
        #endregion

        #region Properties
        public double EstimatedCurrentAbsoluteCameraPose
        {
            get
            {
                double head = (HEAD_PAN_NEUTRAL - this.lastCommandToHeadPanServo)
                    / HEAD_PAN_DEG_COEFFICIENT;
                double body = (BASE_ROTATION_NEUTRAL - this.lastCommandToBaseRotationServo)
                    / BASE_ROTATION_DEG_COEFFICIENT;
                return head + body;
            }
        }

        // MANUAL CONTROL
        public short ManualServoBase { get; set; }
        public short ManualServoShoulder { get; set; }
        public short ManualServoElbow { get; set; }
        public short ManualServoHand { get; set; }
        public short ManualServoPan { get; set; }
        public short ManualServoTilt { get; set; }
        public byte ManualServoSpeed { get; set; }
        public bool ManualControl { get; set; }
        #endregion
    }
}



